/**
* @file DataSet.h
* @author
* @brief 用于各个模块获取共享数据
* @version 1.0
* @date 2020-07-23
*/

#pragma once

#include <cstdint>
#include "data_struct/robot/CliffData.h"
#include "data_struct/robot/MarginSensorData.h"
#include "data_struct/robot/HallSensorData.h"
#include "data_struct/robot/BumperData.h"
#include "data_struct/robot/Pose.h"
#include "data_struct/robot/ImuData.h"
#include "data_struct/robot/OdomData.h"
#include "data_struct/robot/MotorData.h"
#include "data_struct/geometry/PointCloud.h"
#include "data_struct/robot/ObjectData.h"
#include "data_struct/robot/BatteryData.h"
#include "data_struct/robot/OffGroundSwitchData.h"
#include "data_struct/robot/MagneticData.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped.h"

namespace behavior_controller
{
// 机器人当前模式
#define FOLLOW_MODE 0 // 沿边模式 0
#define NAVI_MODE 1   // 导航模式 1
// 前提规划动作
#define WAIT_TIME 0
#define SELF_TEST_PLAN 1
#define MAP_POSE_CHECK_PLAN 2
#define ROOM_DIVIDER_PLAN 3

// 沿边模式下的障碍处理规划动作
#define OBS_FOLLOW 4   // 沿障碍物状态
#define CLIFF_FOLLOW 5 // 沿悬崖状态
// 导航模式下的障碍处理规划动作
#define CLIFF_ACTION 6 // 悬崖响应状态
#define NAVI_ACTION 7  // 导航状态,执行导航操作的状态
// 二级导航标题规划动作
#define NAVI_TO_POSE_PLAN 11
#define COVERAGE_PLAN 12
// 模式通用规划动作
#define STATION_RETURN_PLAN 15          // 返回充电桩响应状态
#define BUMPER_PLAN 16                  // Bumper碰撞规划状态
#define TOP_CAP_OBS_PLAN 17             // 上盖障碍物响应状态
#define SAWTOOTH_WALL_PLAN 18           // 阶跃墙面响应状态
#define TILT_PLAN 19                    // 倾角响应状态
#define SLIP_PLAN 20                    // 打滑响应状态
#define SMALL_OBS_PLAN 21               // 小障碍物响应状态
#define POSITION_STUCK_PLAN 22          // 位置卡死响应状态
#define BRUSH_WINDER_PLAN 23            // 边刷缠绕响应状态
#define FAN_BLOCK_PLAN 25               // 风机堵转响应状态
#define FRONT_PUSHABLE_OBS_PLAN 25      // 前方障碍物响应状态
#define SENSOR_TRIGGER_PLAN 26          // 传感器高频触发响应状态
// 导航任务类型
#define TO_FOLLOW_POINT_AUTO    0
#define TO_FOLLOW_POINT         1
#define TO_SECTION_POINT        2
#define TO_NEXT_ROOM_POINT      3
#define TO_STATION_POINT        4
#define TO_ANY_POINT            5
// 全覆盖执行结果
#define UNFINISHED              0
#define OVER_OBSTACLE           1
#define OVER_SECTION            2
#define OVER_ROOM               3
#define FINISHED                4

namespace DataSet
{
extern int clean_mode;                          // 当前的清洁模式，如导航、沿边
extern int ctrl_state;                          // 当前的控制模块中的规划器选用状态量
extern int ctrl_state_sub;                      // 当前的控制模块中的规划器选用状态量
extern int clean_mode_suspend;                  // 当前的清洁模式，如导航、沿边
extern int ctrl_state_suspend;                  // 当前的控制模块中的规划器选用状态量
extern int ctrl_state_sub_suspend;              // 当前的控制模块中的规划器选用状态量
extern CliffData cliff_data;                    // 悬崖传感器更新的数据
extern BumperData bumper_data;                  // 碰撞传感器更新的数据
extern MarginSensorData margin_data;            // 沿边传感器更新的数据
extern MagneticData magnetic_data;              // 磁感应数据
extern PoseF robot_pose;                        // 机器人的位姿,从定位模块得来
extern OffGroundSwitchData off_ground_switch;   // 离地传感器状态量
extern PointCloud<double> scan_point_cloud;     // 从scan得到的点云数据
extern ImuData imu_data;                        // Imu数据
extern ObjectData object_data;                  // 障碍物位置数据
extern OdomData odom_data;                      // 里程计数据
extern MotorData motor_data;                    // 电机功率、电流等数据
extern BatteryData battery_data;                // 电池数据
};

void updateState(int clean_mode = -1, int ctrl_state = -1, int ctrl_state_sub = -1,
                 int clean_mode_suspend = -1, int ctrl_state_suspend = -1, int ctrl_state_sub_suspend = -1);

geometry_msgs::msg::PoseStamped getRobotPose();

// 参数
extern bool SELT_TEST_RESULT;
extern int SIM_MODE;
extern int USE_ROBOT_FRAME;
extern int FOLLOW_WALL;
extern double ROBOT_RADIUS;    // 机器人机身半径
extern double RADIUS_SQUA;
extern double ROBOT_DIAMETER;  // 机器人机身直径
extern double DIAMETER_SQUA;
extern double MAX_TILT;        // 触发倾角设定,弧度。 4度= 4*3.1415926/180
extern double MIN_TILT;        // 去除触发倾角设定,弧度。 2度= 3*3.1415926/180
extern double MID_TILT;        // 去除触发倾角设定,弧度。 3度= 2*3.1415926/180
extern double CLIFF_MAXDIST;
extern double CLIFF_OBSDIST;   // 需要控制的机器人与悬崖的距离
extern double MARGIN_OBSDIST;  // 需要控制的与障碍物的距离
extern double MARGIN_LEFT;
extern double MARGIN_RIGHT;
extern double MARGIN_MAXDIST;  // 沿边传感器的最大距离
extern double MARGIN_TURNDIST;
extern double MARGIN_P;
extern double MARGIN_I;
extern double MARGIN_D;
// 调试变量
extern double t_linear1;
extern double t_linear2;
extern double t_linear3;
extern double t_angular1;
extern double t_angular2;
extern double t_angular3;
// 激光雷达在机器人上的位置
extern PoseF lidar_pose;
// 沿边传感器
extern PoseF margin_pose;
// 探地传感器在机器人上的位置
extern PoseF camera_pose;
extern PoseF left_fron_cliff;
extern PoseF left_mid_cliff;
extern PoseF left_back_cliff;
extern PoseF righ_fron_cliff;
extern PoseF righ_mid_cliff;
extern PoseF righ_back_cliff;

} // namespace behavior_controller